#!/usr/bin/python
# -*- coding: UTF-8 -*-
import platform
from ctypes import *
import math
import ctypes
import cv2 as cv
import numpy as np

class _callback(object):
  b_device_offline = False

  def device_event_callback(self, devicd_idx, event_id):
    print("get callback event: {} {}" .format(devicd_idx, event_id))
    if event_id == -2001:
      print("=== Event Callback: Device Offline!")
      self.b_device_offline = True
    return 0

  def device_check_status(self):
    if self.b_device_offline == True:
      return False
    else:
      return True

#def register_callback(self, callback):
#	self.cdll.register_callback(callback)
#	return

if platform.system().lower() == 'windows':
  lib = cdll.LoadLibrary('percipio_cam.dll')
else:
  lib = cdll.LoadLibrary('libpercipio_cam.so')
trigger = c_bool(0)
depth = c_bool(1)
color = c_bool(1)
isp_en = c_bool(1)
#INIT DEVICE

b_device_offline = False

ret=lib.ty_camera_init(trigger, depth, color, isp_en)

_cb = _callback()
regCallbackType = CFUNCTYPE(None, c_int, c_int)
cb = regCallbackType(_cb.device_event_callback)
lib.register_callback(cb)

print("sdk init ret = %d" % ret)
#GET DEVICE CNT
cnt=lib.ty_cameraGetCnt()
print("percipio device cnt : %d" % cnt)

color_width = lib.ty_cameraGetRGBImageWidth()
color_height = lib.ty_cameraGetRGBImageHeight()
print("RGB image size :{} {}" .format(color_width, color_height))
depth_width = lib.ty_cameraGetDepthImageWidth()
depth_height = lib.ty_cameraGetDepthImageHeight()
print("Depth image size :{} {}" .format(depth_width, depth_height))

RGB_BUFFER_LEN = color_width*color_height*3
DEPTH_BUFFER_LEN=depth_width*depth_height
DEPTH_BUFFER_LEN2=color_width*color_height

P3D_BUFFER_LEN=depth_width*depth_height*3
P3D_BUFFER_LEN2=color_width*color_height*3

data_rgb = (c_ubyte * RGB_BUFFER_LEN)()
data_rgb2 = (c_ubyte * RGB_BUFFER_LEN)()
data_depth = (c_ushort * DEPTH_BUFFER_LEN)()
data_depth2 = (c_ushort * DEPTH_BUFFER_LEN2)()

data_p3d = (c_float * P3D_BUFFER_LEN)()
data_p3d2 = (c_float * P3D_BUFFER_LEN2)()

depthIntrinsic = (c_float * 9)()
lib.ty_cameraGetDepthCalibIntrinsic(0, depthIntrinsic)
depthIntrWidth = lib.ty_cameraGetDepthCalibIntrinsicWidth(0)
depthIntrHeight = lib.ty_cameraGetDepthCalibIntrinsicHeight(0)
print("Depth image intrinsic info: {} {}".format(depthIntrWidth, depthIntrHeight))
print("    {}    {}    {}".format(depthIntrinsic[0], depthIntrinsic[1], depthIntrinsic[2]))
print("    {}    {}    {}".format(depthIntrinsic[3], depthIntrinsic[4], depthIntrinsic[5]))
print("    {}    {}    {}".format(depthIntrinsic[6], depthIntrinsic[7], depthIntrinsic[8]))
print("==================================================================")

rgbIntrinsic = (c_float * 9)()
lib.ty_cameraGetColorCalibIntrinsic(0, rgbIntrinsic)
rgbIntrWidth = lib.ty_cameraGetColorCalibIntrinsicWidth(0)
rgbIntrHeight = lib.ty_cameraGetColorCalibIntrinsicHeight(0)
print("RGB image intrinsic info: {} {}".format(rgbIntrWidth, rgbIntrHeight))
print("    {}    {}    {}".format(rgbIntrinsic[0], rgbIntrinsic[1], rgbIntrinsic[2]))
print("    {}    {}    {}".format(rgbIntrinsic[3], rgbIntrinsic[4], rgbIntrinsic[5]))
print("    {}    {}    {}".format(rgbIntrinsic[6], rgbIntrinsic[7], rgbIntrinsic[8]))

FrameIdx=0

while(True):
  idx = c_int(0)
  lib.ty_camera_fetch_all_image.restype=ctypes.c_bool
  ret = lib.ty_camera_fetch_all_image(data_rgb, data_depth, idx)
  if ret==False:
    ret = _cb.device_check_status()
    if ret==False:
      cv.destroyAllWindows()
      lib.ty_camera_close()
      print("The program is terminated!")
      exit(0)
  else :
    lib.ty_map_depth_to_point3d(data_depth, depth_width, depth_height, depthIntrinsic, depthIntrWidth, depthIntrHeight, data_p3d)
    offset_center = int(depth_width * depth_height / 2 + depth_width / 2)
    print("Before alignment, the depth map center info(x,y,z):{} {} {}".format(data_p3d[3*offset_center], data_p3d[3*offset_center+1], data_p3d[3*offset_center+2]))

    lib.ty_cameraMapDepthImage2RGBCoordinate(data_depth, depth_width, depth_height, color_width, color_height, data_depth2, idx)
    lib.ty_map_depth_to_point3d(data_depth2, color_width, color_height, rgbIntrinsic, rgbIntrWidth, rgbIntrHeight, data_p3d2)
    offset_center = int(color_width * color_height / 2 + color_width / 2)
    print("After alignment, the depth map center info(x,y,z):{} {} {}".format(data_p3d2[3*offset_center], data_p3d2[3*offset_center+1], data_p3d2[3*offset_center+2]))

    lib.ty_cameraGetUndistortRGBImage(data_rgb, color_width, color_height, idx, data_rgb2)
    flatNumpyArray = np.array(data_rgb2)
    bgrImage = flatNumpyArray.reshape(color_height, color_width, 3)

    flatNumpyArray2 = np.array(data_depth2)
    depthImage = flatNumpyArray2.reshape(color_height, color_width)
    cv.imshow('bgr', bgrImage)
    cv.imshow('depth', depthImage)
    offset_center = int(color_width * color_height / 2 + color_width / 2)
    print("RGBD image center info:{} {} {} {}".format(data_rgb2[3*offset_center + 2], data_rgb2[3*offset_center + 1], data_rgb2[3*offset_center + 0], data_depth2[offset_center]))
    k = cv.waitKey(10)
    if k==ord('q'): 
      break
    FrameIdx+=1

cv.destroyAllWindows()
lib.ty_camera_close()
print("Main done!")

